#include "piswfr.h"

#include "math.h"

#define PI 3.1415926f

/* 机械结构的关节定义位置参照docx文档中所述 */
/** 机械结构定义 单位：mm 注意：必须使用浮点型**/
#define L1 (100.0f) // 左腿大长度
#define L2 (100.0f) // 左腿小长度
#define L3 (100.0f) // 右腿小长度
#define L4 (100.0f) // 右腿大长度
#define L5 (100.0f) // 躯干长度

#define L12 (L1 + L2) // 左腿总长度 a
#define L34 (L3 + L4) // 右腿总长度 b

#define Leftpoint (L5 - L34)                // 左极限点
#define Rightpoint (L1 + L2)                // 右极限点
#define Midpoint (L5 * (L12) / (L12 + L34)) // 最大高度时的交点x坐标位置



#define S_half ((L12 + L34 + L5) / 2)                                                          // 三角形半周长
#define H_max ((2.0f / L5) * (sqrt(S_half * (S_half - L12) * (S_half - L34) * (S_half - L5)))) // 末端的最大高度

#define P_half ((L1 + L4+ (L2+L3-L5))/2) // 三角形半周长
#define H_min ((2.0f / (L2+L3-L5)) * (sqrt(P_half * (P_half - L1) * (P_half - L4) * (P_half - (L2+L3-L5))))) // 末端的最小高度 末端的软件限位高度 我们不允许腿部形状成为一个凹四边形
/**
 * @brief 端点关联极限x to y计算函数
 * @param x 位置x
 * @param x_limit 位置x极限
 * @param y_limit 位置y极限
 * @note 会自动限制x坐标在合理范围内
 *       根据定义的关机与躯干长度，计算出端点的位置极限，当输入的x坐标小于L5/2时 是右腿长度极限圆弧的左半段
 *       当输入的x坐标大于等于L5/2时 是躯干长度极限圆弧的右半段
 *       且x不能小于（L5-（L3+L4）） 不能大于（L1+L2）
 *  */

static void endpoint_y_limit(float x, float *y_limit)
{
    /* 限制x坐标在合理范围 */
    if (x < Leftpoint)
    {
        x = Leftpoint;
    }

    if (x > Rightpoint)
    {
        x = Rightpoint;
    }

    /*只考虑轮子在身体下方的情况 输出值始终为正值*/
    if (x < Midpoint)
    {
        *y_limit = sqrtf((L34) * (L34) - ((x - L5) * (x - L5))); // 右腿长度极限圆弧的左半段
    }
    else if (x > Midpoint)
    {
        *y_limit = sqrtf((L12) * (L12) - (x * x)); // 左腿长度极限圆弧的右半段；
    }
    else if (x == Midpoint)
    {
        *y_limit = H_max;
    }
}

/**
 * @brief 端点关联极限y to x计算函数
 * @param y 位置y
 * @param x_limit_L 位置x极限左
 * @param x_limit_R 位置x极限右
 * @note 会自动限制y坐标在合理范围内
 */
static void endpoint_x_limit(float *y, float *x_limit_L, float *x_limit_R)
{
    /* 限制y坐标在合理范围 */
    if (*y < H_min)
    {
        *y = H_min+1; // 轮子必须处于机身下方
    }
    else if (*y > H_max)
    {
        *y = H_max ; // 不允许轮子超出腿的最大距离
    }
    /*******************/
    if (*y < H_max)
    {
        *x_limit_L = -sqrtf((L34) * (L34) - ((*y) * (*y))) + L5; //
        *x_limit_R = sqrtf(L12 * L12 - ((*y) * (*y)));           //
    }
    else // 浮点数不能使用 == 运算符，所以直接跳转
    {
        *x_limit_L = L5 * (L12) / (L12 + L34); // 交点位置
        *x_limit_R = L5 * (L12) / (L12 + L34); // 交点位置
    }
}

/**
 * @brief 位置向角度的逆解
 *
 * @param x 位置x
 * @param y 位置y
 * @param a 角度a 返回的是弧度值 左腿角度
 * @param b 角度b 返回的是弧度值 右腿角度
 * @param mode 逆解模式 0:x优先模式 1:y优先模式
 * @return void
 */
void inverse_positional_problem(float x, float y, float *anlge_a, float *angle_b, int mode)
{
    if (mode == 0)
    {
        /* x优先模式 以x位置限制y坐标范围 */
        /* 限制y坐标在合理范围 */
        float y_limit;
        endpoint_y_limit(x, &y_limit);
        if (y > y_limit)
        {
            y = y_limit;
        }
    }

    /* y优先模式 以y位置限制x坐标范围 */
    else if (mode == 1)
    {
        float x_limit_L, x_limit_R;
        endpoint_x_limit(&y, &x_limit_L, &x_limit_R);
        if (x < x_limit_L)
        {
            x = x_limit_L;
        }

        if (x > x_limit_R)
        {
            x = x_limit_R;
        }
    }

    float alpha1, alpha2, beta1, beta2;

    float a = 2 * x * L1;
    float b = 2 * y * L1;
    float c = x * x + y * y + L1 * L1 - L2 * L2;

    float d = 2 * (x - L5) * L4;
    float e = 2 * y * L4;
    float f = (x - L5) * (x - L5) + L4 * L4 + y * y - L3 * L3;

    alpha1 = 2 * atan2f((b + sqrtf(a * a + b * b - c * c)), (a + c));
    alpha2 = 2 * atan2f((b - sqrtf(a * a + b * b - c * c)), (a + c));

    beta1 = 2 * atan2f((e + sqrtf(d * d + e * e - f * f)), (d + f));
    beta2 = 2 * atan2f((e - sqrtf(d * d + e * e - f * f)), (d + f));

    alpha1 = (alpha1 >= 0) ? alpha1 : (alpha1 + 2 * PI);//切换到0-2PI范围
    alpha2 = (alpha2 >= 0) ? alpha2 : (alpha2 + 2 * PI);//切换到0-2PI范围

    if (alpha1 >= PI / 4)//0-45度为不合理区间解
    {
        *anlge_a = alpha1;
    }
    else
    {
        *anlge_a = alpha2;
    }

    if (beta1 >= 0 && beta1 <= PI / 4)//0-45度为合理区间解
    {
        *angle_b = beta1;
    }
    else
    {
        *angle_b = beta2;
    }
}

/**
 * @brief 路径生成函数
 * @param x_now 当前位置x
 * @param y_now 当前位置y
 * @param x_des 目标位置x
 * @param y_des 目标位置y
 * @param anlge_a 左腿关节角度
 * @param anlge_b 右腿关节角度
 * @param mode 路径生成模式 0:x优先模式 1:y优先模式
 */
void Path_generation(float x_now, float y_now, float x_des, float y_des, float *anlge_a, float *anlge_b, int mode)
{
    float kp = 1.0f; // 位置误差权重
    float ki = 0.0f; // 位置误差积分权重
    float kd = 0.0f; // 角度误差权重

    float x_s, y_s;

    x_s = x_now + kp * (x_des - x_now); // 当前位置+ 比例*位置误差

    y_s = y_now + kp * (y_des - y_now); //

    inverse_positional_problem(x_s, y_s, anlge_a, anlge_b, mode);
}